#include <windows.h>
#include <tchar.h>
#include <stdio.h>
#include <strsafe.h>
#include "internal.h"
#include "Defines.h"
#include "DeviceProperty.h"
#include "PortableDeviceProps.h"
#include "ObjectLock.h"
#include "SensorDdi.h"
#include "Sensor.h"
#include "SensorRegistry.h"
#include "ReadWriteRequest.h"
#include "AutoResetEvent.h"
#include "RequestContext.h"
#include "Client.h"
#include "DataQueue.h"
#include "Thread.h"
#include "Dummy.h"
#include "ClientsManager.h"
#include "AccelerometerSensor.h"
#include "AccelerometerSensor.tmh"

//
// implement the sensor
//
IMPLEMENT_SENSOR(CAccelerometerSensor);

CAccelerometerSensor::CAccelerometerSensor(void)
{
	//
	// default values read from my WP7Accelerometer; will be
	// used in case "ReadCalibration" fails
	//
	m_x0 = m_y0 = m_z0 = 0x7D;
	m_xg = m_yg = 0x97;
	m_zg = 0x96;

	//
	// by default we use stream I/O to write reports
	//
	m_bUseAltWriteMethod = false;
}

CAccelerometerSensor::~CAccelerometerSensor(void)
{
}

HRESULT CAccelerometerSensor::Init(IWDFDevice *pDevice, CSensorDDI *pSensorDDI)
{
	CObjectLock lock(this);

	//
	// call the base implementation first
	//
	HRESULT hr = CSensor::Init(pDevice, pSensorDDI);
	if(FAILED(hr))
		return hr;

	//
	// add sensor supported properties
	//
	hr = m_Properties.AddProps(DEVPROPS(
		MAKEPROP(SENSOR_PROPERTY_TYPE, SENSOR_TYPE_ACCELEROMETER_3D),
		MAKEPROP(SENSOR_PROPERTY_MANUFACTURER, DEVICE_MANUFACTURER_VALUE),
		MAKEPROP(SENSOR_PROPERTY_MODEL, DEVICE_MODEL_VALUE),
		MAKEPROP(SENSOR_PROPERTY_SERIAL_NUMBER, DEVICE_SERIAL_NUMBER_VALUE),
		MAKEPROP(SENSOR_PROPERTY_FRIENDLY_NAME, L"WP7 Accelerometer Sensor"),
		MAKEPROP(SENSOR_PROPERTY_DESCRIPTION, L"WP7 Accelerometer Sensor"),
		MAKEPROP(SENSOR_PROPERTY_MIN_REPORT_INTERVAL, MINIMUM_REPORT_INTERVAL),
		MAKEPROP(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, DEFAULT_REPORT_INTERVAL),
		MAKEPROP(SENSOR_PROPERTY_CONNECTION_TYPE, SENSOR_CONNECTION_TYPE_PC_ATTACHED),
		MAKEPROP(SENSOR_PROPERTY_PERSISTENT_UNIQUE_ID, GetPUID())
	));

	//
	// add sensor supported data fields
	//
	if(SUCCEEDED(hr))
	{
		hr = m_Data.AddProps(DEVPROPS(
			MAKEPROP(SENSOR_DATA_TYPE_ACCELERATION_X_G, DEFAULT_ACCELERATION_G),
			MAKEPROP(SENSOR_DATA_TYPE_ACCELERATION_Y_G, DEFAULT_ACCELERATION_G),
			MAKEPROP(SENSOR_DATA_TYPE_ACCELERATION_Z_G, DEFAULT_ACCELERATION_G)
		));
	}

	//
	// update WP7 Accelerometer sensor state
	//
	SetState((FAILED(hr) == TRUE) ? SENSOR_STATE_ERROR : SENSOR_STATE_READY);
	return hr;
}

HRESULT CAccelerometerSensor::StartIO(IWDFDevice* pDevice)
{
	CDummy& dummy = CDummy::GetDummy();
	dummy.start();

	return S_OK;
	//
	// initialize read I/O
	//
	HRESULT hr = S_OK;
	for(int i = 0 ; i < READ_CHANNELS_COUNT ; ++i)
	{
		hr = CComObject<CReadWriteRequest>::CreateInstance(&m_pHidReadRequest[i]);
		if(SUCCEEDED(hr))
		{
			m_pHidReadRequest[i]->AddRef();
			hr = m_pHidReadRequest[i]->InitializeRequest(pDevice, this);
			if(SUCCEEDED(hr))
			{
				hr = m_pHidReadRequest[i]->CreateAndSendReadRequest();
			}
		}
	}

	//
	// initialize write I/O
	//
	if(SUCCEEDED(hr))
	{
		hr = CComObject<CReadWriteRequest>::CreateInstance(&m_pHidWriteRequest);
		if(SUCCEEDED(hr))
		{
			m_pHidWriteRequest->AddRef();
			hr = m_pHidWriteRequest->InitializeRequest(pDevice, this);
			if(SUCCEEDED(hr))
			{
				//
				// read accelerometer calibration data
				//
				ReadCalibration();
			}
		}
	}

	return hr;
}

HRESULT CAccelerometerSensor::StopIO(IWDFDevice*)
{
	CDummy& dummy = CDummy::GetDummy();
	dummy.stop();

	return S_OK;

	for(int i = 0 ; i < READ_CHANNELS_COUNT ; ++i)
	{
		m_pHidReadRequest[i]->CancelAndStopPendingRequest();
		m_pHidReadRequest[i]->UninitializeRequest();
		SAFE_RELEASE(m_pHidReadRequest[i]);
	}

	m_pHidWriteRequest->CancelAndStopPendingRequest();
    m_pHidWriteRequest->UninitializeRequest();
    SAFE_RELEASE(m_pHidWriteRequest);

	return S_OK;
}

//
// Accelerometer ID.
//
wstring CAccelerometerSensor::GetId() const
{
	return L"Accelerometer";
}

//
// Accelerometer name.
//
wstring CAccelerometerSensor::GetName() const
{
	return L"Accelerometer";
}

//
// Accelerometer functional category.
//
GUID CAccelerometerSensor::GetFunctionalObjectCategory() const
{
	return SENSOR_CATEGORY_MOTION;
}

//
// Asynchronous read call back function
//
void CAccelerometerSensor::OnAsyncReadCallback(BYTE* buffer, ULONG length, void *context)
{
	UNREFERENCED_PARAMETER(context);

	//
	// NOTE:
	//	Length seems to be zero always even though buffer does
	//	have valid data.  Some inexplicable weirdness!
	//
	//if(length == 0)
	//	return;

	InputReport type = (InputReport)buffer[0];
	Trace(TRACE_LEVEL_INFORMATION, "OnAsyncReadCallback() - type = %d", type);
	switch(type)
	{
	case CAccelerometerSensor::irButtonsAccel:
	case CAccelerometerSensor::irIRAccel:
	case CAccelerometerSensor::irExtensionAccel:
	case CAccelerometerSensor::irIRExtensionAccel:
		{
			ParseAccelerationData(buffer, length);
			break;
		}

	case CAccelerometerSensor::irReadData:
		{
			ParseReadData(buffer, length);
			break;
		}

	case CAccelerometerSensor::irOutputReportAck:
		{
			//
			// this is an acknowledgement for a "CAccelerometerSensor::orWriteMemory"
			// write request; we simply set the write ack event
			//
			if(m_RequestContext.m_Type & CRequestContext::typeWriteMemory)
			{
				m_RequestContext.m_WriteMemory.m_Event.Set();
			}

			break;
		}
	}
}

HRESULT CAccelerometerSensor::WriteReport(BYTE *buffer, ULONG length)
{
	//
	// validate input
	//
	if(buffer == NULL || length <= 0)
		return E_POINTER;

	//
	// if this request is a "WriteMemory" then initialize the
	// write memory context
	//
	if(buffer[0] == (BYTE)CAccelerometerSensor::orWriteMemory)
	{
		if(m_RequestContext.m_WriteMemory.m_Event.Init() == false)
		{
			return HRESULT_FROM_WIN32(GetLastError());
		}

		m_RequestContext.m_Type = (CRequestContext::Type)(m_RequestContext.m_Type | CRequestContext::typeWriteMemory);
	}

	//
	// send the request to the WP7Accelerometer
	//
	HRESULT hr = S_OK;
	if(m_bUseAltWriteMethod == false)
	{
		hr = m_pHidWriteRequest->CreateAndSendWriteRequest(buffer, length);
	}
	else
	{
		hr = m_pHidWriteRequest->CreateAndSendIOCTLRequest(IOCTL_HID_SET_OUTPUT_REPORT, buffer, length);
	}

	if(FAILED(hr))
	{
		return hr;
	}

	//
	// if this request is a "WriteMemory" operation then wait
	// for the acknowledgement to arrive
	//
	if(buffer[0] == (BYTE)CAccelerometerSensor::orWriteMemory)
	{
		m_RequestContext.m_WriteMemory.m_Event.Wait();
	}

	return hr;
}

HRESULT CAccelerometerSensor::ReadMemory(ULONG address, ULONG length, PBYTE buffer)
{
	//
	// initialize a read memory context object
	//
	m_RequestContext.m_ReadMemory.m_ulAddress = address & 0xffff;	// zero out everything in upper 2 bytes
	m_RequestContext.m_ReadMemory.m_ulLength = length;
	m_RequestContext.m_ReadMemory.m_pReadBuffer = new BYTE[length];

	//
	// initialize request buffer
	//
	BYTE request[READ_BUFFER_SIZE] = { 0 };

	//
	// set request type
	//
	request[0] = (BYTE)CAccelerometerSensor::orReadMemory;

	//
	// set address from where memory is to be read
	//
	request[1] = (BYTE)(((address & 0xff000000) >> 24) | RUMBLE_BIT);
	request[2] = (BYTE)((address & 0x00ff0000)  >> 16);
	request[3] = (BYTE)((address & 0x0000ff00)  >>  8);
	request[4] = (BYTE)(address & 0x000000ff);

	//
	// set the number of bytes to read
	//
	request[5] = (BYTE)((length & 0xff00) >> 8);
	request[6] = (BYTE)(length & 0xff);

	//
	// initialize request context
	//
	m_RequestContext.m_Type = CRequestContext::typeReadMemory;

	//
	// write the request to the WP7Accelerometer
	//
	HRESULT hr = WriteReport(request, READ_BUFFER_SIZE);
	if(FAILED(hr))
		return hr;

	//
	// wait for the read operation to complete for 1 second
	//
	if(m_RequestContext.m_ReadMemory.m_Event.Wait(1000) == WAIT_TIMEOUT)
	{
		//
		// free memory;
		//
		delete [] m_RequestContext.m_ReadMemory.m_pReadBuffer;
		m_RequestContext.m_ReadMemory.m_pReadBuffer = NULL;

		return HRESULT_FROM_WIN32(ERROR_TIMEOUT);
	}

	//
	// check if the WP7Accelerometer returned any error
	//
	if(FAILED(m_RequestContext.m_ReadMemory.m_hr))
		return m_RequestContext.m_ReadMemory.m_hr;

	//
	// copy the buffer to the caller's buffer
	//
	CopyMemory(buffer, m_RequestContext.m_ReadMemory.m_pReadBuffer, length);
	return S_OK;
}

void CAccelerometerSensor::ParseAccelerationData(PBYTE buffer, ULONG length)
{
	UNREFERENCED_PARAMETER(length);

	//
	// save the raw acceleration values
	//
	m_rx = buffer[3];
	m_ry = buffer[4];
	m_rz = buffer[5];

	//
	// normalize
	//
	m_fx = (FLOAT)(m_rx - m_x0) / (FLOAT)(m_xg - m_x0);
	m_fy = (FLOAT)(m_ry - m_y0) / (FLOAT)(m_yg - m_y0);
	m_fz = (FLOAT)(m_rz - m_z0) / (FLOAT)(m_zg - m_z0);

	//
	// store the new acceleration data in "m_Data"
	//
	m_Data.SetValue(SENSOR_DATA_TYPE_ACCELERATION_X_G, m_fx);
	m_Data.SetValue(SENSOR_DATA_TYPE_ACCELERATION_Y_G, m_fy);
	m_Data.SetValue(SENSOR_DATA_TYPE_ACCELERATION_Z_G, m_fz);

	//
	// raise a data event if necessary
	//
	CClientsManager& cm = CClientsManager::GetClientsManager();
	if(cm.CanRaiseEvents())
	{
		//
		// construct the properties collection
		//
		vector<CDeviceProperty> props(3);
		props[0] = CDeviceProperty(SENSOR_DATA_TYPE_ACCELERATION_X_G, m_fx);
		props[1] = CDeviceProperty(SENSOR_DATA_TYPE_ACCELERATION_Y_G, m_fy);
		props[2] = CDeviceProperty(SENSOR_DATA_TYPE_ACCELERATION_Z_G, m_fz);

		//
		// raise the event
		//
		cm.PostDataEvent(GetId(), props);
	}
}

void CAccelerometerSensor::ParseReadData(PBYTE buffer, ULONG length)
{
	UNREFERENCED_PARAMETER(length);

	//
	// Error reading data from WP7Accelerometer: Bytes do not exist.
	//
	if((buffer[3] & 0x08) != 0)
	{
		m_RequestContext.m_ReadMemory.m_hr = HRESULT_FROM_WIN32(ERROR_INVALID_ADDRESS);
		m_RequestContext.m_ReadMemory.m_Event.Set();
		return;
	}

	//
	// Error reading data from WP7Accelerometer: Attempt to read from write-only registers.
	//
	if((buffer[3] & 0x07) != 0)
	{
		m_RequestContext.m_ReadMemory.m_hr = HRESULT_FROM_WIN32(ERROR_READ_FAULT);
		m_RequestContext.m_ReadMemory.m_Event.Set();
		return;
	}

	//
	// get our size and offset from the report
	//
	ULONG size = (buffer[3] >> 4) + 1;
	ULONG offset = (buffer[4] << 8 | buffer[5]);

	//
	// add it to the buffer
	//
	memcpy(m_RequestContext.m_ReadMemory.m_pReadBuffer + (offset - m_RequestContext.m_ReadMemory.m_ulAddress),
		   buffer + 6,
		   size);

	//
	// if we've read it all, set the event
	//
	if(m_RequestContext.m_ReadMemory.m_ulAddress + m_RequestContext.m_ReadMemory.m_ulLength == (offset + size))
	{
		m_RequestContext.m_ReadMemory.m_hr = S_OK;
		m_RequestContext.m_ReadMemory.m_Event.Set();
	}
}

void CAccelerometerSensor::ReadCalibration()
{
	BYTE buffer[7];

	//
	// read 7 bytes from memory address 0x0016
	//
	HRESULT hr = ReadMemory(0x0016, 7, buffer);
	if(FAILED(hr) && hr == HRESULT_FROM_WIN32(ERROR_TIMEOUT))
	{
		//
		// try alternate I/O method (use IOCTLs)
		//
		m_bUseAltWriteMethod = true;
		hr = ReadMemory(0x0016, 7, buffer);
	}

	if(SUCCEEDED(hr))
	{
		m_x0 = buffer[0];
		m_y0 = buffer[1];
		m_z0 = buffer[2];
		m_xg = buffer[4];
		m_yg = buffer[5];
		m_zg = buffer[6];
	}
}
